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INTRODUCTION 

In  the  context  of  autonomy  for  underwater  vehicles,  we  assume  that  a  usual  suite  of 
feedback  controllers  are  present  in  the  form  of  autopilot  functions  that  provide  for  the 
regulation  of  vehicle  speed,  heading,  and  depth  or  altitude.  In  this  chapter,  we 
consider  the  topic  of  Guidance  Laws,  Obstacle  Avoidance  and  the  use  of  Artificial 
Potential  Functions  (APFs).  This  topic  deals  with  the  computations  required  to  plan 
and  develop  paths  and  commands,  which  are  used  by  these  auto-pilots.  Simple 
guidance  laws  such  as  “proportional  guidance’  have  been  used  for  many  years  in 
missiles  to  provide  interception  with  targets.  Lateral  accelerations  are  commanded 
proportional  to  the  rate  of  change  of  line  of  sight.  So  long  as  the  chaser  vehicle  has  a 
speed  advantage  over  the  non- maneuvering  target,  simply  reducing  the  angle  of  line 
of  sight  to  zero  will  result  in  an  interception. 

For  applications  with  unmanned  underwater  vehicles,  guidance  laws  allow  vehicles  to 
follow  paths  constructed  in  conjunction  with  mission  objectives.  For  instance,  in  a 
mine-hunting  mission,  we  often  design  paths  that  ‘mow  the  lawn’.  An  objective  area 
is  defined;  tracks  are  developed,  with  track  spacing  defined  according  to  the  swath 
width  of  the  side- scanning  sonar  and  a  required  overlap,  so  that  complete  coverage  of 
the  area  is  obtained.  Increasing  overlap  leads  to  increased  probability  of  detection  at 
the  expense  of  overall  search  rate.  These  tracks  are  defined  by  starting  and  ending 
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points  (way-points)  and  the  vehicle  is  guided  along  these  tracks  by  a  track  following 
guidance  law  that  drives  the  cross-track  position  error  to  zero.  In  this  mode,  the 
vehicle  closely  follows  the  defined  path  in  position  only,  but  no  attempt  is  made  to 
follow  a  specific  trajectory  in  both  time  and  position.  Trajectory  tracking  (Kaminer, 
et.  al.,  1998)  which  may  be  necessary  if  formation  control  is  required,  implies  that  not 
only  cross-track  but  also  the  corresponding  along  track  errors  are  controlled.  Clearly 
this  added  requirement  means  that  some  position  along  the  track  determined  by  some 
moving  point  on  the  trajectory  is  to  be  maintained.  In  essence  trajectory  following  is 
more  complex  and  requires  speed  as  well  as  heading  control.  Another  related  topic 
involves  the  design  of  optimal  paths  to  follow  for  example  in  (Milam,  2003)  .  In  this 
sense,  path  length  is  minimized  subject  to  curvature  and  other  constraints  rather  than 
allowing  the  vehicle  to  respond  to  way-points,  with  no  guaranteed  coverage  in  tight 
spaces  between  closely  positioned  points.  In  open  ocean  conditions  it  is  rare  that  such 
precision  in  an  actual  path  is  important,  but  it  could  be  important  in  obstacle 
avoidance  maneuvering  where  minimum  distances  between  vehicles  and  obstacles 
should  be  observed. 

By  reference  to  the  Figure  1  below,  the  auto -pilots  for  Speed  Heading  and  Depth 
control  are  assumed  to  be  already  present  in  the  vehicle  dynamics.  The  notion  of  a 
guidance  system  is  based  around  the  idea  that  the  heading  command  is  taken  from  the 
guidance  system.  It  allows  for  track  following,  cross  track  error  control  and  for 
obstacle  avoidance,  and  as  such  requires  knowledge  of  vehicle  position  from  a 
navigation  system,  and  knowledge  of  the  mission  so  that  track  plans  can  be  made  and 
modified.  In  the  discussion  of  obstacle  avoidance  guidance,  we  include  the  methods 
of  artificial  potential  fields,  curved  path  deviation  planning,  and  reactive  avoidance. 


The  system  is  driven  from  the  perceptory  inputs  modified  by  sonar  signal  processing, 
and  algorithms  for  obstacle  detection,  obstacle  tracking,  location  and  mapping. 


VEHICLE  GUIDANCE,  TRACK  FOLLOWING 

To  follow  a  set  of  straight  line  tracks,  forms  the  basis  of  many  simple  guidance 
requirements.  In  this  section,  a  simple  line  of  sight  (LOS)  guidance  law  is  described. 
It  is  supplemented  with  a  simple  cross  track  error  term  for  better  performance  in  cross 


Autonomous  Underwater  Vehicle  Guidance  System 


Figure  1  Overview  of  Vehicle  Guidance  Functions 


currents,  and  a  sliding  mode  controller  is  presented  that  has  been  experimentally 
validated  under  a  wide  variety  of  conditions.  Other  works  have  studied  similar 
problems  for  land  robots,  (for  example,  smooth  path  planning  in  Kanayama  and 
Hartman,  1997)  and  usually  develop  a  stable  guidance  law  based  on  cross  track  error 
which  returns  the  vehicle  to  the  desired  path. 


In  all  simulations  shown  later  in  this  chapter,  the  same  vehicle  dynamics  model  is 
used,  taken  from  the  REMUS  AUV  as  explained  in  earlier  work  (Fodrea  and  Healey, 
2003)  and  the  steering  auto-pilot  is  designed  according  to  a  sliding  mode 
methodology  (Marco  and  Healey,  2001).  The  hydrodynamic  coefficients  are  given  in 
(Fodrea  and  Healey,  2003. 


Vehicle  Steering  Model 

Using  the  coefficients  given  below,  and  with  standard  nomenclature  for  underwater 
vehicles  (Healey,  Class  Notes,  2001), 
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Table  of  Data  for  the  REMUS  Model  Use  in  Simulations 


Kr 

-3.55exp01  kg 

K 

1.93  kg  m/rad 

Y 

vr 

-6.66exp01  kg/s  (Same  as  Z„) 

Y 

r 

2.2  kg  m/s  (Same  as  Zq) 

N- 

vr 

1.93  kg  m 

Nr 

-4.88  kg  m2/rad 

NVr 

-4.47  kg  m/s 

Nr 

-6.87  kg  m2/s  (Same  as  Mq) 

-3.46exp0 1/3.5  kg  m/s2 

5.06exp01/3.5  kg  m/s2 


m=  30.48  kg  I7/=  3.45  kg  m2/rad 

Table  of  Hydrodynamic  and  Inertial  Parameters  for  REMUS  Steering  Dynamics 
with  the  modifications  to  the  values  taken  initially  from  Prestero,  (2001), 


and  in  more  compact  form 


x(t)  =  Ax(  t )  +  bu(  t ) 


(2) 


with  the  path  response  being  taken  from 


X=UQ  cosy  -  vr  sirty  +  Ucx 
Y  =  UQ  siny  +  v,.cc«\|/  +  U cy 

and 
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where  x(t)  =  \ yr  r  y];u(t)  =  8r(t )  (4) 

In  the  above,  Ucx  and  Ucy  are  Northerly  and  Easterly  water  currents  respectively.  With 
this  in  mind,  any  steering  control  law  may  be  used,  but  for  the  sake  of  further 
discussion  lets  use  the  sliding  mode  control, 


5 r(t)  =  Klv(t)  +K2r{t)  +r|  tanh(o(t)  /<>); 

a(t)  =  sl(yco^OSii))-y(t))+s2(rcom(t)-r(t))-s3v(ty, 

where,  for  the  REMUS  vehicle  model,  the  constants,  based  on  pole  placement  are 
taken  to  be:  s ,=0.8647  ;s2=0.5004  ;  s,  =  0.0000;  r|  =0.5000  ;  <)>  =  0.1000  ;  K,  = 


0.0000  ;K2  =  0.6000  ; 


(6) 


The  coefficients  of  the  side  slip  velocity  have  been  zeroed  since  it  is  not  practical  to 
include  that  particular  sensor  in  the  control  because  of  noise  levels,  and  it  has  been 
found  that  the  stability  of  the  heading  controller  is  not  impaired. 


Line  of  Sight  Guidance 


The  basic  guidance  law  is  then  given  in  terms  of  the  Line  of  Sight  Law  for  use  with 
the  autopilot  in  Eq.(6)  is  then 


V(Ocom(i.os(g)  tan  (X(t)wpt^  /  X(t)wpt^)  (7) 

Y(tlptli)=(Ywpl<i)-Y(t)) 

X(tlP,(i)=(Xwp,(i)-X(t)) 

The  position  definitions  and  nomenclature  are  as  given  by  reference  to  Figure  2. 


N,  X 


Figure  2.  Cross  Track  Position  Error  e(t)  Definitions 


A  typical  response  for  the  vehicle  track  following  in  the  presence  of  Northerly  and 
Easterly  currents  of  one  half  a  knot  is  illustrated  in  Figure  3.  In  this  figure  we  see  that 
the  vehicle  heads  along  the  track  but  is  seriously  in  error  with  the  presence  of  the 
cross  currents. 


Waypoint  Following  with  0.5  Knot  South  Westerly  Current 


Figure  3,  Fine  of  Sight  Guidance  Only  in  the  Presence  of  0.5  Knot  Cross  Currents 


Cross  Track  Error 

The  variable  of  interest  to  minimize  is  the  cross  track  error,  E(t) ,  and  is  defined  as  the 
perpendicular  distance  between  the  center  of  the  vehicle  (located  at  (X(t),Y(t) )  and 
the  adjacent  track  line.  The  total  track  length  between  way  point  i  and  i-1  is  given  by 


L;  = 


ft 


X  —  X 

wpt(  i )  xvpt( 


(Y  -Y 

V  wpt(i)  wpt( 


^ y 


(8) 


where  the  ordered  pairs  (X  (i),  Y  (i))  and  (  Y 


X 


wpt(  i-1 ) f  xvpt(  i-1 ) 


)  are  the  current  and 


previous  way-points  respectively.  The  track  angle,  v| ftrk(j),  is  defined  by 


(9) 


¥mt(i)  tan  2  (Ywpt<i)  Ywpt(ilyX  wpt(i^  Xwpt<il)). 


The  cross  track  heading  error  \j7  ( /  )  C7/,  f , )  for  the  fh  track  segment  is  defined  as 


V(0  CTE(i)=V(t)-Vlrk(n  +PW 


(10) 


where  'tf(t)clE(tj  must  be  normalized  to  lie  between  ±  180°.  [)(/)  is  the  angle  of  side 
slip  and  is  defined  here  as 

tan($(i))=v(t)/U 

When  \j/ (t)CTEU)  is  nulled,  it  may  be  seen  that  the  instantaneous  velocity  vector  then 
heads  along  the  track  with  i(/(/)=i|/,rfOI-P(f) 

The  difference  between  the  current  vehicle  position  and  the  next  way  point  is 

x(t)wpl(i)=  xwpl(i)  -  x(t)  (11) 

Y( 0wpt(i)  =  Ywpt(i)  -  Y(t) 

With  the  above  definitions,  the  distance  to  the  ith  way  point  projected  to  the  track  line 
S(t)j ,  can  be  calculated  as  a  percentage  of  track  length  using 


S(  t  \  ( X  ( t  )wpt(i)(  Xwpt<  Xwpt<il))  +  Y  (  t  )\vpt(  i  )  (  Ywpt(  i )  YWpt(  j-_i  j  ))/L,  (12) 


The  cross  track  error  E(t)  may  now  be  defined  in  terms  of  the  current  vehicle  position, 
the  previous  and  next  waypoints  as 


e(t)  =  S.(  t  )tan(  d(t )) 

i  p 


(13) 


where  d  (t)  is  the  angle  between  the  line  of  sight  to  the  next  way  point  and  the 
current  track  line  given  by 


d At)  =  tan2~'  (Yt 


—  V  Y  —  Y  ) 

wpt( i )  1  wpt(  i—1 ^  wpt(  i  )  wpt(  i—1 )  / 


-tan2~‘  (Y  ( t ) 


wpt(  i ) 


'  X  ( t)wpt(i  >) 


(14) 


and  must  be  normalized  to  lie  between  ±  1 80° . 


Line  of  Sight  with  Cross  Track  Error  Controller 


Cross  track  error  is  controlled  by  the  addition  of  a  term  in  the  guidance  law 
proportional  to  cross  track  error  which  provides  additional  heading  to  drive  to  the 
track  path  in  spite  of  cross  currents.  This  topic  is  discussed  in  detail  in  Papoulias 
(1995). 


VtfWos®)  =  tanl(Y(t)wm/ X(t)wpm)-tanl(E(t)/  p)  (15) 


In  the  Cross  Track  Error  guidance  law  above,  p  is  a  parameter  usually  chosen 


depending  on  the  vehicle  turning  radius  and  is  commonly  between  4-5  vehicle 


lengths.  Too  small  a  value  leads  to  unstable  steering  response.  It  should  be  pointed  out 
that  in  dealing  with  real  time  control  applications  with  unmanned  underwater 
vehicles,  the  issues  surrounding  the  wrapping  of  the  heading  are  troublesome.  For 
control  and  guidance  work  we  generally  bound  headings  to  the  sector 

-180  <\| /  <180 

although  for  filtering  work  the  heading  state  has  to  be  continuous  and  wrapped.  Figure 
4  shows  an  improved  tracking  result  from  this  guidance  law,  even  in  the  presence  of 
cross  currents.  Steady  state  errors  persist  but  can  be  eliminated  by  an  integral  of  error 
term  if  so  desired,  Figure  5.  Notice  in  Figure  5  that  there  is  an  over  and  undershoot 
behavior  with  integral  control  even  though  it’s  magnitude  is  limited  using  anti-reset 
windup  techniques. 


Y  (m) 


Figure  (4)  Line  of  Sight  with  Simple  Cross  Track  Error  Term 
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Figure  5  Line  of  Sight  with  Cross  Track  Error  Term  And  Integral  Of  Error  In  The 

Presence  Of  Cross  Currents 

One  of  the  problems  with  this  approach  is  that  stability  is  not  guaranteed  as  part  of  the 
design  process  and  depends  on  the  selection  of  the  look  ahead  distance  p. 


Sliding  Mode  Cross  Track  Error  Guidance: 


With  the  cross  track  error  defined,  a  sliding  surface  can  be  cast  in  terms  of  derivatives 
of  the  errors  such  that 

e(f)  =  e(f) 

£  (^  —  [/sin(\|7(f)CJE(;)) 

(  1 

£(0  =  U  (r(0+P  (0)cos(\|7  (t)CTg!)) 

e(f)  =  tz (/( t)+\i (r))cos(i|7 ( t)crF(l) )  -  f/(r(0+P (t)f  sin(\)7 (t)crlci)) 


The  question  now  arises  as  to  how  to  include  the  effects  of  the  side  slip  and  its  rate  of 
change.  We  generally  make  the  assumption  that  the  time  rate  of  change  of  side  slip  is 
small  compared  to  the  turn  rate,  r(t),  although  this  depends  somewhat  on  the 
particular  vehicle  considered.  For  the  NPS  ARIES  vehicle,  it  is  typically  less  than  0. 1 


rad/sec.  and  may  be  neglected  Thus  we  assume  that  f3(7),  and  [)(  t )  in  Equations 
(16)  are  negligible. 

A  sliding  surface  for  the  cross  track  error  controller  is  selected  to  be  a  second  order 
polynomial  of  the  form 

a  (?)  =  z(t)  +  X,e(t)  +  X2e(t)  (17) 

The  reaching  condition  for  reduction  of  error  is 

d(  t  )=e(  t)  +  'kle(t)  +  'k2e(t)  =  -r\  tanh(c/§  ),  (18) 

and  to  recover  the  input  for  control,  the  heading  dynamics  Equation  (1)  are  simplified 
to  neglect  side  slip  dynamics  reducing  to 

r(  t )  =  ar(  t)  +  bhr(t)\  \| f(t)  =  r(t); 

and  substituting  into  (18)  using  (16)  we  get  an  expression  for  d(  t ): 

U  (, ar(t )  +  bdr)  cos(\ |7  (t)cn:{l) )  -  Ur(t)2  sin(y(t)CTE(i) )  +  X,Ur(t)  cos(\\f  (t)clE<i) ) 

+  ^2  U  sin{\\f  (t)(77 'E(i)  ) 

Rewriting  Equation  (17),  the  sliding  surface  becomes 


a  (0  =  U  tit) cos (7 (0 CTE(i) )  +  \U  sin(7(t)CIEU))  +  X2e ( t ) . 


(20) 


The  rudder  input  can  then  be  expressed  using  (19)  and  (18), 


'  -Uar(t)  cos  (7  (t)CTE(i)) 

+  U (r(t))2  sin(7(t)CTE(i))-'klUr(t)cos(7(t) CT£(; , )  > 
-  X2U  sin (7 (t) CTE(i ) )  -  ?  tanh(c (t)/<\> ) 

(21) 

where  X,  =  0.6,  X2  -  0.1,  x |  =  0. 1 ,  and  ())  =  0.5.  To  avoid  division  by  zero,  in 
the  rare  case  where  cos{7{t)CTE)  =  0.0  (i.e.  the  vehicle  heading  is  perpendicular  to 

the  track  line)  the  rudder  command  is  set  to  zero  since  this  condition  is  transient  in 
nature. 

Large  Heading  Error  Mode 

Heading  errors  larger  than  say  40  degrees  often  occur  in  operations,  especially  when 
transitioning  between  tracks.  It  is  not  uncommon  for  a  90  degree  heading  change  to  be 
instituted  suddenly.  Under  these  conditions,  the  Sliding  Mode  Guidance  Law  breaks 
down,  and  a  return  to  the  basic  Line  of  Sight  Control  Law  is  required.  In  this 
situation,  the  heading  command  can  be  determined  from 

v(0com(tosm)  =tanTl(Y(t)wpKiyX(t)wpKi))  (22) 

and  the  LOS  error  from 

V  (0  LOS(i )  — V  (t\om(  LOS(i ))  ~  V  (0  ’  @3) 

and  the  control  laws  used  for  heading  control,  Equations  (5,6)  may  be  used. 


5  At)  = 


1 


Ubcos(7(t)CTE(i) ) 


Track  Path  Transitions: 


Based  on  operational  experience,  we  find  it  best  to  seek  a  combination  of  conditions 
in  order  for  the  way  point  index  to  be  incremented.  The  first  and  most  usual  case  is  if 
the  vehicle  has  penetrated  the  way  point  watch  radius  R  . ) .  This  is  likely  if  the 

vehicle  is  tracking  well  on  its  present  track  with  small  cross  track  error.  Secondly, 
most  usually  at  start  up,  if  a  large  amount  of  cross  track  error  is  present,  the  next  way 
point  will  become  active  if  the  projected  distance  to  the  way  point  S(t)l  reached  some 
minimum  value  Smin(i) ,  or  in  simple  cross  track  error  guidance,  that  the  projected 
distance  S(t),  becomes  less  than  the  cross  track  error  control  distance,  p,  This  is 
encoded  with  the  transition  conditional, 

if  Utii'Kvf +(?(»„«„)’*  Km  I'  S(«,<  1 1  S «,  <  P )  THEN 

Activate  Next  Way  Point 

OBSTACLE  AVOIDANCE 


Three  forms  of  obstacle  avoidance  guidance  are  considered.  Firstly,  when  an  obstacle 
is  detected  using  the  forward  looking  sonar  perception  system,  some  metrology 
analysis  will  give  size  information  and  if  it  is  determined  to  be  an  object  to  be  avoided 
by  steering,  a  planned  path  deviation  can  be  used  to  override  the  current  path 
requiring  the  vehicle  to  follow  a  deviated  path.  Criteria  are  then  checked  to  see  if  it  is 
safe  to  return  to  the  original  path.  Avoidance  paths  can  be  evaluated  using  many 


possibilities  but  circular  paths  are  convenient.  Planning  and  replanning  takes  place 
continuously  until  no  further  need  to  avoid  is  reached  when  the  original  path  is 
regained.  In  the  deviation  plan,  an  added  heading  command  is  prescribed  for  the 
vehicle  to  follow  a  circular  path  until  a  sufficient  deviation  from  the  obstacle  is 
reached  at  which  time,  the  avoidance  plan  is  ended,  returning  the  vehicle  to  the 
original  path  following  behavior. 

Planned  Avoidance  Deviation  in  Path 

In  this  section  a  proposed  planned  path  is  determined  from  a  circular  path  that 
deviates  the  heading  50  degrees  off  course  over  the  planning  horizon  of  20  meters. 
This  is  followed  by  a  linear  segment  until  a  predetermined  cross  track  deviation  is 
made.  This  planned  approach  has  the  advantage  that  a  predetermined  deviation  from 
path  is  made  before  the  vehicle  is  released  to  return  to  the  original  path.  The  amount 
of  deviation  from  the  original  path  is  determined  from  the  characteristics  as  well  as 
the  position  of  the  obstacle  as  seen  by  the  forward-looking  sonar.  The  generation  of 
the  planned  deviated  path  is  triggered  by  the  sonar  detection  of  an  obstacle  coming 
within  the  planning  horizon  (in  this  case  20  meters)  and  at  a  bearing  within  a  planning 
sector  of  45  degrees.  The  offset  of  the  object  from  the  path  is  sensed  and  used  in  the 
calculation  of  the  avoidance  distance  needed. 

A  predetermined  deviation  is  described  using  the  following  pseudocode 

if  (Range  (i,  1 )  <20  &  abs  (Bearing  (i,  1)  )  <  pi/4 
&  inplan==0) ,  inplan=l ; end; 

if  (inplan  ==1), 

count=count+l ; 


offset=Range (i,  1)  *sin (Bearing (i,  1) ) ; 


%replace  heading  command  with  path  planning 
command  with  Radius  R=20m,  until  50  degrees 
deviation  is  met  followed  by  a  straight  line 
until  offset  criterion  is  met; 

psi_errorLOS (i) =  psi_comLOS (i)  -  psi_cont (i) 

+  (count) *dt*U/R; 

Xdev (i) =R*sin  (count *dt *U/20)  ; 

Ydev (i) =R* (1 -cos (count *dt *U/20) ) ; 
end; 

%  limit  the  heading  deviation 

if  ( ( (count)  *dt *U/R)  >  50*pi/180)  , 
psi_errorLOS (i) =  psi_comLOS  (i)  -  psi_cont  (i) 

+  50*pi/180; 

Xdev  (i)  =Xdev  (i-1)  +dt  *U*sin  (50*pi/180)  ; 

Ydev  (i )  =Ydev  (i-1 )  ++dt  *U*sin  (50*pi/180)  ; 
end; 

if  (abs  (cte  (i) )  >15) , 

inplan=0 ;  count=0;  r_com  (i)  =0 ;  end; 

%  back  to  default  plan 


Obstacle  Run 


Figure  6  Response  of  Vehicle  to  a  Planned  Deviation  in  Track 


Many  different  varieties  of  planned  paths  are  possible.  The  above  is  only  an  example. 
The  point  is  that  using  different  planning  methods,  the  paths  selected  are  guaranteed 
to  be  free  of  obstacles.  Planning  for  each  segment  of  travel,  and  considering  whether 
or  not  to  deviate  the  path  can  be  done  continually  in  sequence  every  20  meters  or  so. 
If  subsequent  obstacles  are  found  ,  deviational  paths  can  be  instituted  as  needed. 


Figure  7  below  shows  the  response  of  the  vehicle  when  a  second  obstacle  is  detected 
while  returning  to  the  original  track  from  an  initial  deviation. 
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Figure  7  Multiple  Obstacles  Using  the  Planned  Avoidance  Deviation  Method 

Reactive  Avoidance 

Reactive  avoidance  is  based  on  the  processing  of  the  forward  look  sonar  image  and 
detection  of  obstacles  from  which  an  avoidance  or  threat  level  can  be  assessed.  Based 
on  the  threat  level  the  strength  of  the  avoidance  action  is  determined  to  be  either 
strong  or  weak.  The  response  is  immediate  and  hence  is  called  reactive. 

The  model  uses  a  two-dimensional  forward-looking  sonar  with  a  120°  horizontal 
scan  and  a  110- meter  radial  range.  The  probability  of  detection  is  based  on  a  cookie- 
cutter  approach  in  which  the  probability  of  detection  is  unity  within  the  scan  area  and 
zero  anywhere  else.  Bearing  is  resolved  to  the  nearest  degree  and  range  is  resolved 
every  20  centimeters. 

The  obstacle  avoidance  model  developed  in  this  work  is  based  on  the  product  of 


bearing  and  range  weighting  functions  that  form  the  gain  factor  for  a  dynamic 


obstacle  avoidance  behavior.  The  basis  for  the  weighting  functions  lies  in  a  fuzzy 
logic  methodology.  The  weighting  functions  are  MATLAB  membership  lunctions 
from  the  fuzzy  logic  toolbox  with  the  parameters  selected  to  maximize  obstacle 
avoidance  behavior.  The  membership  function  for  bearing  is  a  Gaussian  curve 
function  of  the  form: 

-( *  -  c )2 

w1=e(2a2)  (25) 

where  the  parameters  x,  c,  and  a  are  position  (or  angular  position  in  degrees  for  the 
purpose  of  this  model),  center,  and  shape  respectively.  Shape  defines  the  steepness  of 
the  Gaussian  curve.  The  values  selected  for  these  parameters  to  provided  sufficient 
tuning  in  this  membership  function  were  -90:90,  0,  20  respectively.  The  bearing 
weighting  function  can  be  seen  in  Figure  8  below. 


Bearing  Weighting  Function 


Figure  8.  Bearing  Weighting  Function 


The  membership  function  for  range  is  an  asymmetrical  polynomial  spline -based 
curve  called  zmf  and  is  of  the  form: 

w2-  zmf(x,[a  b])  (26) 

where  a  and  b  are  parameters  that  locate  the  extremes  of  the  sloped  portions  of  the 
curve.  These  parameters  are  called  breakpoints  and  define  where  the  curve  changes 
concavity.  In  order  to  maximize  obstacle  avoidance  behavior,  these  values  were 
tuned  to  be  ( sonrange-99 )  and  ( sonrange-90 ).  With  this  selection,  the  range  weight  is 
approximately  unity  for  anything  closer  than  20  meters  and  zero  for  anything  farther 
than  40  meters  from  REMUS  as  seen  in  Figure  9  below. 


Range (m) 


Figure  9.  Range  Weighting  Function, 

A  final  weight  based  on  both  bearing  and  range  is  calculated  from  the  product  of 
wl  and  w2 .  This  weight  becomes  the  gain  coefficient  that  is  applied  to  a  maximum 
avoidance  heading  for  each  individual  object.  The  maximum  heading  is  K  /  4  as  seen 
below: 


Voa(.t,c)  =  wlw2( 71/4) 


(27) 


where  t  is  the  time  step  and  c  is  the  obstacle  being  evaluated.  The  avoidance  heading 
for  all  obstacles  over  a  single  time  step  (or  one  look)  is  then 

Voatooi(0=&oa(^)  (28) 

i 

Following  an  evaluation  of  each  obstacle  at  every  time  step,  a  final  obstacle 
avoidance  heading  term  is  determined  from  the  sum  of  the  obstacle  avoidance 
headings  of  each  individual  object  within  a  specified  bearing  and  range  from  the 
vehicle  or 

Yooto,(  =  (29) 

cc 

where  cc  is  the  counter  used  to  determine  how  many  obstacles  fall  into  this  window. 
The  counter  is  used  to  normalize  this  overall  obstacle  avoidance  term  to  an  average 
for  all  of  the  obstacles  within  the  range  above.  This  bearing  and  range  of  the  window 
is  determined  through  a  rough  evaluation  of  the  weighting  functions.  In  order  to  fall 
into  the  window,  the  gain  factor  must  be  equal  to  or  exceed  a  value  of  Hqw2=0.15. 

The  obstacle  avoidance  term  \|/oato/f)  is  then  incorporated  into  vehicle  heading 
error  as: 

¥toS(0  =Ym«* (0  -Yco„,(0  -  -  (30) 

arctan(e(0/p)+¥oaM(0 

This  heading  error  drives  the  rudder  commands  to  maneuver  around  detected 
objects  in  the  track  path. 

The  initial  test  performed  on  the  two-dimensional  sonar  model  was  navigation 
around  a  single  point  obstacle.  This  is  the  simplest  obstacle  avoidance  test  for  the  2-D 


model.  Two  variations  of  this  test  were  run  for  the  basic  single  point  obstacle 


avoidance:  a  single  point  on  the  path  and  a  single  point  off  the  path. 
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Figure  10a.  Single  Point  Obstacle  Run  Centerline 


Figure  10b.  Single  Point  Obstacle  Run  Rudder/FleadingA|/oa 


Rudder-r  (deg),  Heading-b(deg),  Psioatot-g(deg) 


Figure  10c.  Zoom  of  Figure  10b 
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Figure  1 1  Single  Point  Obstacle  Run  Off  Path 


Figures  10b  shows  the  rudder  dynamics,  vehicle  heading,  and  obstacle  avoidance 
heading  term  for  the  duration  of  each  vehicle  run.  The  rudder  action  has  a  direct 
correlation  with  the  obstacle  avoidance  heading  and  overall  vehicle  heading.  The 
large  angle  motions  of  the  heading  are  the  ninety-degree  turns  made  to  track  the 
ordered  vehicle  path.  There  is  an  associated  rudder  action  with  each  of  these  turns  as 
seen  by  the  corresponding  rudder  curve.  These  rudder  curves  show  that  the  maximum 
programmable  rudder  deflection  is  9°.  For  all  dynamic  behaviors,  whether  associated 
with  a  turn  or  obstacle  avoidance  maneuver,  the  rudder  initiates  the  turn  with  this 
maximum  value.  In  order  to  regain  track,  the  rudder  action  may  vary. 

A  single  point  obstacle  avoidance  model  is  far  simpler  than  a  multiple  point 
obstacle  avoidance  model  not  only  in  the  maneuvering  of  the  vehicle,  but  also  in 
maintaining  the  obstacle  picture.  For  multiple  point  obstacle  avoidance,  it  is 
necessary  to  have  a  model  that  reacts  to  obstacles  in  a  certain  proximity  to  its  path 
rather  than  all  possible  obstacles  seen  by  the  sonar  scan.  Weighting  functions  allow 
for  an  accurate  compilation  of  this  obstacle  picture.  The  REMUS  model  builds  an 
obstacle  counter  for  obstacles  having  a  weighting  function  gain  factor  (w iwi)  greater 
than  0.15.  This  value  allows  for  a  maximum  rudder  and  bearing  weight  of 


approximately  0.386,  the  square  root  of  0.15.  Referring  to  the  membership  functions 
in  Figures  8  and  9,  a  value  of  0.386  correlates  to  a  bearing  and  range  of  approximately 
+/-300  and  30  meters  respectively. 

As  seen  in  the  following  figures,  REMUS  successfully  avoids  multiple  points  and 
multiple  point  clusters  in  the  same  fashion  it  avoided  a  single  points.  The  rudder 
dynamics  are  minimal  during  all  avoidance  maneuvers  for  an  efficient  response. 


Figure  12  Multiple  Point  Obstacle  Run 
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Figure  13  Multiple  Cluster  Obstacle  Run 


As  seen  in  Figure  14  below,  an  obstacle  appearing  in  the  vehicle  path  causes  the 
vehicle  heading  to  deviate  from  its  track  path  heading  of  90°  approximately  the  same 
amount  as  the  obstacle  avoidance  heading. 
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Figure  14.  Vehicle  Heading  Comparison  Avoidance  Command  and  Heading 

Response 

ARTIFICIAL  POTENTIAL  FUNCTIONS 

The  use  of  Artificial  Potential  Functions  in  Vehicle  Guidance  is  useful  in  both 
directing  and  redirecting  vehicle  paths  as  well  as  in  the  avoidance  of  mapped 
obstacles.  Essentially,  the  local  areas  in  which  the  vehicle  is  being  guided  is  mapped 
into  a  potential  field  such  that  heading  commands  are  derived  from  the  shaping  of 
these  potentials.  In  this  manner,  obstacles  to  be  avoided  are  associated  with  high 


potential,  free  spaces  are  represented  by  the  valleys  and  the  vehicle  guidance  seeks 
minima  in  the  potential  field.  The  motion  of  the  vehicle  is  taken  to  be  the  motion  of  a 
particle  moving  along  potential  lines.  In  this  work  we  have  considered  the  use  of 
Gaussian  potential  functions  to  provide  smooth  path  following  where  the  paths 
derived  are  continuous  with  continuous  derivatives  and  curvature.  This  is  different 
from  others  such  as  Khatib,  1986,  for  instance,  who  used  inverse  radius  potential 
functions  with  singularities  at  the  obstacle  locations.  In  all  potential  function  based 
guidance  laws,  two  potentials  must  be  employed,  one  to  follow  the  desired  path  or 
track  and  one  to  avoid  any  obstacles  in  the  way.  While  the  concepts  are  quite  useful, 
in  some  cases  instabilities  can  arise  where  potential  functions  combine  with  vehicle 
response  lags  to  produce  overall  instability  of  path  following.  It  should  be  intuitive 
since  the  potential  and  hence  the  guidance  heading  commands  are  derived  from 
position  based  functions.  Figure  1  shows  the  clear  role  of  vehicle  position  in 
determining  the  heading  command  leading  to  unstable  performance  in  the  guidance 
loop  if  commands  for  reasonable  turn  rates  are  not  considered  carefully.  That  is, 
instabilities  are  possible  and  smooth  paths  with  curvature  limits  must  be  generated. 
The  guidance  laws  are  derived  by  the  following  considerations. 

Potential  Function  for  Obstacle  Avoidance: 

Let  us  define  a  potential  function  within  the  local  region  Q  in  which  V  has  maxima 
located  at  the  points  of  obstacles  to  be  avoided:  It  assumes  that  a  Perception  System 
has  detected  obstacles  and  located  them  so  that  the  locations  are  known. 


V(Z,T)>0  V(X,Y)eQ 


(31) 


If  there  are  obstacles  within  the  region  at  x,. ,  yt ,  then  high  potentials  are  assigned  to 
those  points,  for  example, 

v  (0  =  £  Vi  exp[((X  (t)  -  xf  +  (7(0  -  >',)2)  /  2a,2  ];  (32) 

i 

A  Guidance  Law  to  minimize  the  potential  function  is  obtained  from  seeking  V  to  be 
always  reducing  along  trajectories  in  (X,  7).  We  make 

—  =  V'X  +  V^T<  0  VX,7ef2 
dt 

Leading  to 

X=-yV;  ;  7  =  - yV'y;  y>0 

and  assuming  that  the  vehicle  path  is  commanded  to  follow  tangent  paths  to  the 
potential  lines,  a  heading  command  for  obstacle  avoidance  using  the  artificial 
potential  field  can  be  extracted  using 

voaop/  =tm~l(v;/v;) 

Normalized  Negative  Gradient  of  Potential  Function  vs  x/sigma 
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Figure  15  Negative  Potential  Gradient ,  -V  xVs.  Normalized  Distance  from  Center, 


(33) 


(34) 


(35) 


Sigma  =  1 .0, 


Since  V’  is  negative,  the  avoidance  heading  steers  away  from  the  obstacle.  Also  in 
Figure  15  above,  it  may  be  seen  that  the  gradient  is  zero  at  the  origin  which  does  not 
cause  a  problem  since  the  origin  is  an  unstable  saddle  point  and  paths  always  steer 
away  from  the  obstacle. 

Multiple  Obstacles 

With  multiple  obstacles  present,  each  one  has  a  similar  potential  that  is  summed  to  the 
total.  This  gives  the  more  general  form  for  the  avoidance  command. 

£ (Y(t) -  y, )  /a,2  * V,.  exp[((X  (t)  -  xtf  +  (Y(t)  -  y,  )2)  /  2a,2 ] 

Voaapf  (0  =  tair‘  -F -  (36) 

£  (X(t)  /a,2  *Vt  exp[((A(t)-  x;)2  +  (Y(t)~  yf)/ 2a 2] 

i= 1 

An  example  of  three  obstacles  in  the  field  used  in  simulation  from  above  is  shown  in 
Figure  16. 
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Figure  16,  Potential  Function  Plotted  Vertically  in  the  Y  vs  X  Field,  a  =  3  and  10m. 


This  is  not  particularly  satisfactory  alone  since  the  vehicle  is  already  following  some 
defined  track  and  that  track  has  also  to  be  maintained  in  the  absence  of  obstacles.  The 
problem  is  solved  by  the  addition  of  a  track  following  potential  Vtrack(i }  -  (3f  1  -  Sj . )  in 

the  direction  of  the  current  (?)  track  heading.  The  total  guidance  potential  becomes  the 
sum  of  the  obstacle  avoidance  and  the  track  following  potentials. 


V, 


track  ( i ) 


P*  =  P  COS(  V  track,  i  ,)’  P,=P  Sinty  ,mck( ; }  ) 


so  that  the  total  potential  is  expressed  as  V  —  Vtrack  +  Voa . 


(37) 


This  leads  to  the  expression  for  heading  command  that  includes  the  track  following  as 
well  as  the  obstacle  avoidance  commands  from  the  potential  gradients  as  in  the 
heading  eror  for  the  autopilot  \]7  (t) 


V  (0  =  ^tracku)  -V  (0)  -  tan  (£  /  p )  W 


oaapf  ' 


y oaapf  (0  =  tan  ((V'  +  (3y)/(lC,  +  P,))-X|/ 


(38) 


track  (j) 


The  resulting  heading  controller  will  judiciously  avoid  objects  while  maintaining 
track.  Figure  17  shows  the  result  for  three  obstacles  placed  around  the  planned  tracks 
of  the  vehicle  in  which  two  choices  for  the  parameter  P  are  made. 


APF . 


Avoidance  using  0.05  for  Track  Weight 
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Figure  17  Path  responses  With  [3  =0.05  and  0.1  and  a  =  10m., 

Decreasing  P  in  general  allows  weaker  track  following  when  presented  with  obstacles, 
increasing  P,  results  in  stricter  track  following  and  less  avoidance.  While  there  are  no 
rules  for  the  selection  of  P ,  it  would  be  of  interest  to  relate  its  choice  to  the  minimum 
avoidance  range.  In  general,  it  can  be  said  that  P,  a  gradient,  should  be  scaled 
according  to  the  selection  of  the  inverse  of  the  standard  deviation,  a  in  the  Gaussian 
potentials  .  Since  ct,  is  a  critical  distance,  it  should  be  chosen  with  respect  to  the 
turning  radius  of  the  vehicle.  With  that  said,  a  choice  of  P  at  0. 1  or  0.05  is  in  order. 
Where  a,  is  between  3  and  10  meters.  Since  the  potential  gradient  is  used  as  the  signal 
for  heading  change  the  sharper  the  gradient  the  sharper  the  avoidance  turn  so  that 
small  0i,  leads  to  sharper  turns,  and  larger  0V  lead  to  softer  turns. 
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Figure  18  Obstacle  Avoidance  Gaussian  Potential  Function  with  a  =  3  m. 


Figure  18  shows  the  avoidance  response  using  (3=0. 1  and  a=3m.  in  which  a 
responsive  deviation  is  obtained.  The  minimum  avoidance  distance  for  this  case  is 
about  4  meters.  Varying  a  produces  deviations  that  increase  the  initial  reaction  to 
avoid  with  increasing  a.  However,  it  is  generally  found  that  decreasing  a  allows  more 
overall  path  deviation.  Thus  APFs  can  be  tuned  to  optimize  the  path  deviation  while 
providing  a  minimum  avoidance  radius. 


Oxiac*  ^jn 


Figure  19.  Deviation  Paths  for  a  =  5,7,10,  and  15m.  Showing  Smoother  Paths  with 

Less  Deviation  as  o  Increases 

The  results  of  the  above  simulation  have  indicated  that  APFS  can  be  tuned.  In  the  case 
of  Gaussian  APFs,  the  tuning  parameters  are  the  standard  deviation  and  the  track 
potential  [1.  These  methods  can  also  be  embedded  into  planned  avoidance  functions 
since  the  standard  deviations  and  [3  values  can  be  selected  based  on  a  balancing 
between  path  deviation  and  minimum  avoidance  distances. 

CONCLUSION 


In  this  chapter,  guidance  laws  for  track  following,  cross  track  error  control,  and 
obstacle  avoidance  using  reactive,  planned  and  artificial  potential  function  methods 


have  been  discussed.  While  more  work  remains  to  be  done,  these  methods  of 


avoidance  guidance  form  the  basis  of  most  of  the  needed  algorithms.  The  use  of  APFs 
opens  up  the  possibilities  of  an  optimized  avoidance  plan  once  obstacles  are  detected. 
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